#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64
import math

class MyController:
    def __init__(self):
        rospy.init_node("my_controller", anonymous=True)
        
        # 参数配置
        self.wheel_base = 0.3  # 轴距（前后轮中心距离），需根据实际机器人调整
        self.steering_limit = math.pi / 4  # 转向角限制（±45度）
        self.drive_gain = 1.0  # 驱动力增益（需调优）

        # 初始化订阅者和发布者
        self.cmd_vel_sub = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback)
        
        # 前轮转向控制器发布者
        self.front_left_steering_pub = rospy.Publisher(
            "/robot1/front_left_steering_controller/command", Float64, queue_size=10
        )
        self.front_right_steering_pub = rospy.Publisher(
            "/robot1/front_right_steering_controller/command", Float64, queue_size=10
        )

        # 前轮驱动控制器发布者
        self.front_left_drive_pub = rospy.Publisher(
            "/robot1/front_left_drive_controller/command", Float64, queue_size=10
        )
        self.front_right_drive_pub = rospy.Publisher(
            "/robot1/front_right_drive_controller/command", Float64, queue_size=10
        )

    def cmd_vel_callback(self, msg):
        # 提取线速度和角速度
        vx = msg.linear.x
        wz = msg.angular.z

        # 计算前轮转向角度（阿克曼转向几何）
        if abs(vx) > 1e-3:
            theta = math.atan(self.wheel_base * wz / vx)
        else:
            # 静止时，根据角速度设置最大转向角
            theta = math.copysign(self.steering_limit, wz)

        # 限制转向角在机械范围内
        theta = max(-self.steering_limit, min(self.steering_limit, theta))

        # 计算驱动指令（简化为线速度比例）
        drive_effort = self.drive_gain * vx

        # 发布转向角度
        self.front_left_steering_pub.publish(theta)
        self.front_right_steering_pub.publish(theta)

        # 发布驱动指令（假设左右轮速度相同）
        self.front_left_drive_pub.publish(drive_effort)
        self.front_right_drive_pub.publish(drive_effort)

if __name__ == "__main__":
    try:
        controller = MyController()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass